﻿#ifndef KALMANFILTER_H
#define KALMANFILTER_H

#include "point.h"
#include <QDebug>

class KalmanFilter {
public:
    // 卡尔曼滤波NLOS结构体
    typedef struct {
        float rk_, rk;
        float d_rk_, d_rk;
        float dt, ek;
        float E, K_a, K_b, KkHPk_a, KkHPk_b;
        float P[2][2];
        float AP_AT[4];
        float Q_Dr, Q_r, R;
        unsigned char ErrCount;
    } NlosTypeDef;

    // 卡尔曼滤波实时跟踪结构体
    typedef struct {
        float xk_, xk, yk_, yk;
        float Vxk_, Vxk, Vyk_, Vyk;
        float Q_x, Q_y, Q_Vx, Q_Vy, R_x, R_y;
        float dt, dt_2, AP_AT[16];
        float K[4][2], aPlusR_xDivdetA, eDivdetA, bDivdetA, fPlusR_yDivdetA, detA;
        float ek[2], KkHPk_[4][4];
        float P[4][4];
        unsigned char ErrCount;
    } RtTracingTypeDef;

    //  融合卡尔曼滤波结构体
    typedef struct {
        float xk[2];
        float P[2][2], K[2][2], ek[2];
        float Q_x, Q_y, R_x, R_y;
        float dPlusR_yDivdetA, bDivdetA, cDivdetA, aPlusR_xDivdetA, detA;
        float KkHPk_[2][2];
    } BlendTypeDef;

    KalmanFilter();
    ~KalmanFilter();

    void NLOS(int Zk, int* result);
    void rtTracing(const Point& input, Point* result);
    void blend(const Point& predict, const Point& measure, Point* result);

private:
    void initNLOS();
    void initRtTracing();
    void initBlend();

    NlosTypeDef nlos_p_ = {};
    RtTracingTypeDef rt_p_ = {};
    BlendTypeDef blend_p_ = {};
};

#endif // KALMANFILTER_H
